/*
 * @Author: your name
 * @Date: 2022-04-13 11:14:15
 * @LastEditTime: 2022-04-13 22:30:07
 * @LastEditors: Please set LastEditors
 * @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 * @FilePath: /catkin_wss/src/sub_lidarbox/src/pub_lidarbox_node.cpp
 */
#include "ros/ros.h" //包含了使用ROS节点的必要文件

//visualization_msgs
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
//pcl
#include <pcl/point_types.h>

#include <stdio.h>
#include <iostream>
#include <string>

ros::Publisher pub_lidarBox;
bool isused = true;
int frame_count = 0;

void lidarBoxCallback(){
    visualization_msgs::MarkerArray markerArray;
//    {
//        //marker 1
//        visualization_msgs::Marker marker;
//        marker.header.frame_id = "lidar_link";
//        marker.header.stamp = ros::Time::now();
//        marker.ns = "lidarBox";
//        marker.id = 0;
//        marker.type = visualization_msgs::Marker::CUBE;
//        marker.action = visualization_msgs::Marker::ADD;
//        marker.pose.position.x = 2;
//        marker.pose.position.y = 2;
//        marker.pose.position.z = 2;
//        marker.pose.orientation.x = 0.0;
//        marker.pose.orientation.y = 0.0;
//        marker.pose.orientation.z = 0.0;
//        marker.pose.orientation.w = 1.0;
//        marker.scale.x = 0.5;
//        marker.scale.y = 0.5;
//        marker.scale.z = 0.5;
//        marker.color.a = 1.0;
//        marker.color.r = 1.0;
//        marker.color.g = 0.0;
//        marker.color.b = 0.0;
//        markerArray.markers.push_back(marker);
//    }
//    {
//        //marker
//        visualization_msgs::Marker marker;
//        marker.header.frame_id = "lidar_link";
//        marker.header.stamp = ros::Time::now();
//        marker.ns = "lidarBox";
//        marker.id = 1;
//        marker.type = visualization_msgs::Marker::CUBE;
//        marker.action = visualization_msgs::Marker::ADD;
//        marker.pose.position.x = 4;
//        marker.pose.position.y = 4;
//        marker.pose.position.z = 2;
//        marker.pose.orientation.x = 0.0;
//        marker.pose.orientation.y = 0.0;
//        marker.pose.orientation.z = 0.0;
//        marker.pose.orientation.w = 1.0;
//        marker.scale.x = 0.5;
//        marker.scale.y = 0.5;
//        marker.scale.z = 0.5;
//        marker.color.a = 1.0;
//        marker.color.r = 1.0;
//        marker.color.g = 0.0;
//        marker.color.b = 0.0;
//        markerArray.markers.push_back(marker);
//    }
     if (isused && frame_count > 4)
     {
         frame_count = 0;
         {
             //marker 1
             visualization_msgs::Marker marker;
             marker.header.frame_id = "lidar_link";
             marker.header.stamp = ros::Time::now();
             marker.ns = "lidarBox";
             marker.id = 0;
             marker.type = visualization_msgs::Marker::CUBE;
             marker.action = visualization_msgs::Marker::ADD;
             marker.pose.position.x = 0.392560;
             marker.pose.position.y = 1.658952;
             marker.pose.position.z = 2;
             marker.pose.orientation.x = 0.0;
             marker.pose.orientation.y = 0.0;
             marker.pose.orientation.z = 0.0;
             marker.pose.orientation.w = 1.0;
             marker.scale.x = 0.194273;
             marker.scale.y = 0.167082;
             marker.scale.z = 0.5;
             marker.color.a = 1.0;
             marker.color.r = 1.0;
             marker.color.g = 0.0;
             marker.color.b = 0.0;
             markerArray.markers.push_back(marker);
         }
         {
             //marker
             visualization_msgs::Marker marker;
             marker.header.frame_id = "lidar_link";
             marker.header.stamp = ros::Time::now();
             marker.ns = "lidarBox";
             marker.id = 1;
             marker.type = visualization_msgs::Marker::CUBE;
             marker.action = visualization_msgs::Marker::ADD;
             marker.pose.position.x = 0.592480;
             marker.pose.position.y = 1.655050;
             marker.pose.position.z = 2;
             marker.pose.orientation.x = 0.0;
             marker.pose.orientation.y = 0.0;
             marker.pose.orientation.z = 0.0;
             marker.pose.orientation.w = 1.0;
             marker.scale.x = 0.190870;
             marker.scale.y = 0.187435;
             marker.scale.z = 0.5;
             marker.color.a = 1.0;
             marker.color.r = 1.0;
             marker.color.g = 0.0;
             marker.color.b = 0.0;
             markerArray.markers.push_back(marker);
         }
//         {
//             // marker
//             visualization_msgs::Marker marker;
//             marker.header.frame_id = "lidar_link";
//             marker.header.stamp = ros::Time::now();
//             marker.ns = "lidarBox";
//             marker.id = 2;
//             marker.type = visualization_msgs::Marker::CUBE;
//             marker.action = visualization_msgs::Marker::ADD;
//             marker.pose.position.x = 6;
//             marker.pose.position.y = 7;
//             marker.pose.position.z = 6;
//             marker.pose.orientation.x = 0.0;
//             marker.pose.orientation.y = 0.0;
//             marker.pose.orientation.z = 0.0;
//             marker.pose.orientation.w = 1.0;
//             marker.scale.x = 0.5;
//             marker.scale.y = 0.5;
//             marker.scale.z = 0.5;
//             marker.color.a = 1.0;
//             marker.color.r = 1.0;
//             marker.color.g = 0.0;
//             marker.color.b = 0.0;
//             markerArray.markers.push_back(marker);
//         }
//         {
//             // marker
//             visualization_msgs::Marker marker;
//             marker.header.frame_id = "lidar_link";
//             marker.header.stamp = ros::Time::now();
//             marker.ns = "lidarBox";
//             marker.id = 3;
//             marker.type = visualization_msgs::Marker::CUBE;
//             marker.action = visualization_msgs::Marker::ADD;
//             marker.pose.position.x = 5;
//             marker.pose.position.y = 3;
//             marker.pose.position.z = 2;
//             marker.pose.orientation.x = 0.0;
//             marker.pose.orientation.y = 0.0;
//             marker.pose.orientation.z = 0.0;
//             marker.pose.orientation.w = 1.0;
//             marker.scale.x = 0.0;
//             marker.scale.y = 0.0;
//             marker.scale.z = 0.0;
//             marker.color.a = 1.0;
//             marker.color.r = 1.0;
//             marker.color.g = 0.0;
//             marker.color.b = 0.0;
//             markerArray.markers.push_back(marker);
//         }
     }
     isused = !isused;
     frame_count++;

    pub_lidarBox.publish(markerArray);
}

int    main(int argc, char **argv)
{
    ///////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////ROS
    ros::init(argc, argv, "pub_lidarbox_node");                                      //初始化ROS，节点名命名为node_a，节点名必须保持唯一
    ros::NodeHandle n;                                                    //实例化节点, 节点进程句柄
    ros::Rate loop_rate(30);                                              //设置发送数据的频率为10Hz
    pub_lidarBox = n.advertise<visualization_msgs::MarkerArray>("/objseg_gridclass/LidarObjBox", 1);
    while (ros::ok())
    {
        lidarBoxCallback();
        ros::spinOnce();   //不是必须，若程序中订阅话题则必须，否则回掉函数不起作用。
        loop_rate.sleep(); //按前面设置的10Hz频率将程序挂起
    }
    return 0;
}